//
// Created by jm on 2021/5/28.
//

#ifndef DMPLANNER_H
#define DMPLANNER_H

#include "dmp_graph_search.h"
#include <map/grid_map.h>
/**
 * @brief Abstract base for planning
 */
namespace sz{

class DMPlanner {
public:
  /**
   * @brief Simple constructor
   * @param verbose enable debug mode
   */
  explicit DMPlanner(bool verbose = false);

  /**
   * @brief set a prior path and get region around it
   * @param path prior path
   * @param r radius in x-y plane
   * @param h height in z axis, if 2d, it's not used
   * @param dense if true, dont need to do RayTrace
   *
   * it returns the inflated region
   */

  std::vector<bool> SetPath(const VectorPoint2i &path, const Point2f &radius,
                            bool dense);

  /// Set search radius around a prior path
  void SetSearchRadius(const Point2f& r);
  /// Set potential radius
  void SetPotentialRadius(const Point2f& r);
  /// Set the range of potential zslam_map, 0 means the whole zslam_map
  __attribute__((unused)) void SetPotentialMapRange(const Point2f& r);
  /// Set heuristic weight
  __attribute__((unused)) void SetEps(float eps);
  /// Set collision cost weight
  __attribute__((unused)) void SetCollisionWeight(float c);
  /// Set the power of potential function \f$H_{MAX}(1 - d/d_{GetMax})^{pow}\f$
  void SetPow(int pow);

  /**
   * @brief Status of the planner
   *
   * 0 --- exit normally;
   * -1 --- no path found;
   * 1, 2 --- start or goal is not free.
   */
  int Status() const;
  /// Get the raw path
  VectorPoint2f GetRawPath();
  /// Get the raw path
  VectorPoint2i GetMapRawPath();
  /// Get the prior path
  __attribute__((unused)) VectorPoint2i GetPriorPath();
  /// Get the nodes in open set
  __attribute__((unused)) VectorPoint2f GetOpenSet() const;
  /// Get the nodes in close set
  __attribute__((unused)) VectorPoint2f GetCloseSet() const;
  /// Get all the nodes
  __attribute__((unused)) VectorPoint2f GetAllSet() const;
  /// Get the searching region
  __attribute__((unused)) VectorPoint2f GetSearchRegion();
  /// Get the internal zslam_map zslam_util
  __attribute__((unused)) std::shared_ptr<sz::NavigationMap> GetMapUtil();

  /**
   * @brief Generate distance zslam_map
   * @param map_util MapUtil that contains the zslam_map object
   * @param pos center of the distance zslam_map
   *
   * it copies the zslam_map object, thus change the original map_uitl won't affect
   * the internal zslam_map.
   */
  void SetMap(const std::shared_ptr<sz::NavigationMap> &map_util,
              const Point2f& pos);

  /// Compute the optimal path
  bool Plan(const Point2f& start, const Point2f& goal, const VectorPoint2i &path);
protected:
  /// Create the mask for potential distance field
  VectorEigen<std::pair<Pointi<2>, uint8_t>> CreateMask(int pow);
  /// Need to be specified in Child class, main planning function
  bool Plan(const Point2f &start, const Point2f &goal,
            float_decimal_t eps = 1, float_decimal_t cweight = 0.1);
  /// check availability
  bool CheckAvailability(const Pointi<2> &pn);

  /// Assume using 2D Grid zslam_map
  std::shared_ptr<sz::NavigationMap> map_util_;
  /// The planner back-end
  std::shared_ptr<DMPGraphSearch> graph_search_;
  /// tunnel for visualization
  std::vector<bool> search_region_;
  /// 1-D zslam_map array
  std::vector<uint8_t> cmap_;

  /// Enabled for printing Info
  bool planner_verbose_;
  /// Path cost (raw)
  float path_cost_{};
  /// Prior path from planner
  VectorPoint2i prior_path_;
  /// Raw path from planner
  VectorPoint2i map_raw_path_;
  /// Raw path from planner
  VectorPoint2f raw_path_;
//  /// Modified path for future usage
//  VectorPoint2f path_;
  /// Flag indicating the success of planning
  int status_ = 0;
  /// GetMax potential value
  int8_t H_MAX{100};
  /// heuristic weight
  float eps_{0.0};
  /// potential weight
  float cweight_{0.1};
  /// radius of distance field
  Point2f potential_radius_{Point2f::Zero()};
  /// radius of searching tunnel
  Point2f search_radius_{Point2f::Zero()};
  /// xy range of local distance zslam_map
  Point2f potential_map_range_{Point2f::Zero()};
  /// power index for creating mask
  int pow_{1};

};


class __attribute__((unused)) IterativeDMPlanner : public DMPlanner {
 public:
  __attribute__((unused)) explicit IterativeDMPlanner(bool verbose = false);

  /// Iteratively compute the optimal path
  __attribute__((unused)) bool iterativeComputePath(const Point2f &start, const Point2f &goal,
                                                    const VectorPoint2i &prior_path, int max_iteration);
};


}




#endif
